
#include <gl/glut.h>
#include "GLUtil.h"
#include <vec.h>
#include "Polyhedron.h"
#include "RigidBody.h"
#include <vector>
#include <malloc.h>
#include <math.h>
#include "ConvertUtil.h"
#include "quaternion.h"
#include <objLoader.h>

using namespace std;

#pragma comment(lib, "imager")


static GLint    ImageWidth=800;
static GLint    ImageHeight=800;

bool compute = true;

double colorScale = 0.1;
int kLevel = 25;
double* colorBuffer;

RigidBody* mine;

double xInf, yInf, zInf, xSup, ySup, zSup;

int xRes, yRes, zRes;

double xStep, yStep, zStep;

double mapDepth(double v)
{
	v *= -10;
	//printf("%f\n",v);
	return sqrt(exp(v)) * colorScale;
}

void TrenderMesh()
{
	int w = xRes + 1;

	int x, y;

	x = y = 0;

	Value v;

	int k = kLevel;	

	//glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

	glBegin(GL_TRIANGLES);

	for (double i=xInf; i<xSup; i+=xStep)
	{
		y = 0;

		for (double j=yInf; j<ySup; j+=yStep)
		{
			Vec3d va(i - xStep / 2, j - yStep / 2, 0);
			Vec3d vb(i + xStep / 2, j - yStep / 2, 0);
			Vec3d vc(i + xStep / 2, j + yStep / 2, 0);
			Vec3d vd(i - xStep / 2, j + yStep / 2, 0);

			if (compute)
			{
				v = mine->sdm->getEvaluation(Vec3d(va[0], va[1], zInf + k * zStep), false);
			}			
			v.d = mapDepth(v.d);
			glColor3d(v.d, 0, 1);
			glVertex3d(va[0], va[1], va[2]);
			if (compute)
			{
				v = mine->sdm->getEvaluation(Vec3d(vb[0], vb[1], zInf + k * zStep), false);
			}			
			v.d = mapDepth(v.d);
			glColor3d(v.d, 0, 1);
			glVertex3d(vb[0], vb[1], vb[2]);
			if (compute)
			{
				v = mine->sdm->getEvaluation(Vec3d(vc[0], vc[1], zInf + k * zStep), false);
			}			
			v.d = mapDepth(v.d);
			glColor3d(v.d, 0, 1);
			glVertex3d(vc[0], vc[1], vc[2]);


			if (compute)
			{
				v = mine->sdm->getEvaluation(Vec3d(va[0], va[1], zInf + k * zStep), false);
			}			
			v.d = mapDepth(v.d);
			glColor3d(v.d, 0, 1);
			glVertex3d(va[0], va[1], va[2]);
			if (compute)
			{
				v = mine->sdm->getEvaluation(Vec3d(vc[0], vc[1], zInf + k * zStep), false);
			}			
			v.d = mapDepth(v.d);
			glColor3d(v.d, 0, 1);
			glVertex3d(vc[0], vc[1], vc[2]);
			if (compute)
			{
				v = mine->sdm->getEvaluation(Vec3d(vd[0], vd[1], zInf + k * zStep), false);
			}			
			v.d = mapDepth(v.d);
			glColor3d(v.d, 0, 1);
			glVertex3d(vd[0], vd[1], vd[2]);
		}

		x++;
	}

	//compute = false;

	glEnd();


}

void TrenderVector()
{
	Value v;

	glColor3d(1, 1, 1);

	for (double i=xInf; i<xSup; i+=xStep)
	{
		for (double j=yInf; j<ySup; j+=yStep)
		{
			for (double k=zInf; k<zSup; k+=zStep)
			{
				Vec3d start(i, j, k);

				v = mine->sdm->getEvaluation(start, true);

				Vec3d end = v.n * 1 + start;

				if (v.d > 0)
				{
					continue;
				}

				glBegin(GL_LINES);

				glVertex3d(start[0], start[1], start[2]);
				glVertex3d(end[0], end[1], end[2]);

				glEnd();
			}
		}
	}

	glPointSize(5);

	glColor3d(1, 0, 0);

	for (double i=xInf; i<xSup; i+=xStep)
	{
		for (double j=yInf; j<ySup; j+=yStep)
		{
			for (double k=zInf; k<zSup; k+=zStep)
			{
				Vec3d start(i, j, k);

				v = mine->sdm->getEvaluation(start, true);


				if (v.d > 0)
				{
					continue;
				}

				glBegin(GL_POINTS);

				glVertex3d(start[0], start[1], start[2]);				

				glEnd();
			}
		}
	}
}

void TinitializeMesh(Edge** edgeList, Vert** vertList, Face** faceList, int* faceCount, int* vertCount)
{
	conversionFromObj( edgeList, vertList, faceList, faceCount, vertCount );
}

void _main()
{	

	xRes = 20;
	yRes = 20;
	zRes = 20;

	xInf = -14;
	yInf = -14;
	zInf = -14;

	xSup = 14;
	ySup = 14;
	zSup = 14;

	xStep = (xSup - xInf) / (xRes - 1);
	yStep = (ySup - yInf) / (yRes - 1);
	zStep = (zSup - zInf) / (zRes - 1);
	

	mine = new RigidBody("platform.obj", 1, 0.1);

	for (int i=0; i<3; i++)
	{
		for (int j=0; j<3; j++)
		{
			printf("%f ", mine->bodyInertiaMatrix(i, j));
		}
		printf("\n");
	}

	Value v = mine->sdm->getEvaluation(Vec3d(0,0,0), true);

	//colorBuffer = (double*)malloc((xRes + 1) * (yRes + 1) * sizeof(double));

	printf("%f\n", v.d);

	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);
	glutInitWindowPosition(100, 100);
	glutInitWindowSize(ImageWidth, ImageHeight);
	glutCreateWindow("FLIP");
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutKeyboardFunc(keyboard);

	glutMainLoop();

}

